#include <motor/servo_kinco.h>

namespace ros_canopen_driver {

ServoKinco::ServoKinco(std::shared_ptr<rclcpp::Node> node, const std::string name)
{
    node_ = node;
    joint_name = name;
    servo_manufacturer = KINCO;
    memset(&m_servo_motor, 0, sizeof(SERVOMOTOR_PORT));
    // pCanopen = Canopen::getInstance();
    preTargetEnc = -1;
    motorState = Off;
    pc_.timestamp = node_->now().seconds();
}

ServoKinco::~ServoKinco()
{
    // delete pCanopen;
    // pCanopen = nullptr;
}

/*Description: load motor configuration parameters from yaml file*/
bool ServoKinco::Motor_InitParam()
{
    if (!node_->has_parameter(joint_name + "/nodeId"))
    {
        RCLCPP_ERROR_STREAM(node_->get_logger(), joint_name << " nodeId is invaild");
        return false;
    }
    int nodeId;
    node_->get_parameter(joint_name + "/nodeId", nodeId);
    m_servo_motor.cfg.nodeId = nodeId;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" nodeId is " << nodeId);

    node_->get_parameter_or(joint_name + "/busname", busname, std::string("can0"));
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" busname: "<<busname);

    node_->get_parameter_or(joint_name + "/type", servo_type, 0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" type: "<<servo_type);

    node_->get_parameter_or(joint_name + "/ratio", ratio, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" ratio: "<<ratio);

    node_->get_parameter_or(joint_name + "/pos_scale", pos_scale, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" pos_scale: "<<pos_scale);

    node_->get_parameter_or(joint_name + "/pos_offset", pos_offset, 0.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" pos_offset: "<<pos_offset);

    node_->get_parameter_or(joint_name + "/Ipeak", ipeak, 48.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" Ipeak: "<<ipeak);
    
    node_->get_parameter_or(joint_name + "/use_extra_encoder", use_extra_encoder, false);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" use external encoder: "<<use_extra_encoder);

    node_->get_parameter_or(joint_name + "/rad_to_meter", rad_to_meter, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" rad_to_meter: "<<rad_to_meter);

    node_->get_parameter_or(joint_name + "/zero_point_correction", zero_point_correction, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name<<" zero_point_correction: "<<zero_point_correction);
    /* <--- add end */

    int ctl_mode;
    node_->get_parameter_or(joint_name + "/ctl_mode", ctl_mode, 0);
    m_servo_motor.cfg.ctl_mode = ctl_mode;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" ctl_mode is "<< ctl_mode);

    node_->get_parameter_or(joint_name + "/rotateReverse", m_servo_motor.cfg.rotateReverse, false);

    node_->get_parameter_or(joint_name + "/reduction_ratio", m_servo_motor.cfg.reduction_ratio, 1.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" reduction_ratio is "<< m_servo_motor.cfg.reduction_ratio);

    node_->get_parameter_or(joint_name + "/encoder_resolution", m_servo_motor.cfg.encoder_resolution, 10000.0);
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" encoder_resolution is "<< m_servo_motor.cfg.encoder_resolution);

    // velocity ctl mode cfg
    int velocity_mode_acc;
    node_->get_parameter_or(joint_name + "/vModeCfg/acc", velocity_mode_acc, 0);
    m_servo_motor.cfg.vModeCfg.acc = velocity_mode_acc;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" velocity_mode_acc is "<< m_servo_motor.cfg.vModeCfg.acc);

    int velocity_mode_dec;
    node_->get_parameter_or(joint_name + "/vModeCfg/dec", velocity_mode_dec, 0);
    m_servo_motor.cfg.vModeCfg.dec = velocity_mode_dec;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" velocity_mode_dec is "<< m_servo_motor.cfg.vModeCfg.dec);

    // position ctl mode cfg
    int position_mode_vel;
    node_->get_parameter_or(joint_name + "/pModeCfg/vel", position_mode_vel, 0);
    m_servo_motor.cfg.pModeCfg.vel = position_mode_vel;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" position_mode_vel is "<< m_servo_motor.cfg.pModeCfg.vel);

    int position_mode_acc;
    node_->get_parameter_or(joint_name + "/pModeCfg/acc", position_mode_acc, 0);
    m_servo_motor.cfg.pModeCfg.acc = position_mode_acc;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" position_mode_acc is "<< m_servo_motor.cfg.pModeCfg.acc);

    int position_mode_dec;
    node_->get_parameter_or(joint_name + "/pModeCfg/dec", position_mode_dec, 0);
    m_servo_motor.cfg.pModeCfg.dec = position_mode_dec;
    RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" position_mode_dec is "<< m_servo_motor.cfg.pModeCfg.dec);

    //home cfg
    node_->get_parameter_or(joint_name + "/homeNeed", m_servo_motor.cfg.homeNeed, false);
    if (m_servo_motor.cfg.homeNeed)
    {
        int home_mode;
        node_->get_parameter_or(joint_name + "/hModeCfg/home_mode", home_mode, 0);
        m_servo_motor.cfg.hModeCfg.home_mode = home_mode;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" home_mode is "<< home_mode);

        int home_mode_velH;
        node_->get_parameter_or(joint_name + "/hModeCfg/velH", home_mode_velH, 0);
        m_servo_motor.cfg.hModeCfg.velH = home_mode_velH;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" home_mode_velH is "<< m_servo_motor.cfg.hModeCfg.velH);

        int home_mode_velL;
        node_->get_parameter_or(joint_name + "/hModeCfg/velL", home_mode_velL, 0);
        m_servo_motor.cfg.hModeCfg.velL = home_mode_velL;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" home_mode_velL is "<< m_servo_motor.cfg.hModeCfg.velL);

        int home_mode_acc;
        node_->get_parameter_or(joint_name + "/hModeCfg/acc", home_mode_acc, 0);
        m_servo_motor.cfg.hModeCfg.acc = home_mode_acc;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" home_mode_acc is "<< m_servo_motor.cfg.hModeCfg.acc);

        node_->get_parameter_or(joint_name + "/hModeCfg/homeOffset", m_servo_motor.cfg.hModeCfg.homeOffset, 0);
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_name <<" homeOffset is "<< m_servo_motor.cfg.hModeCfg.homeOffset);
    }
   
    return true;
}

/*Description: motor init(include motor enable,gohome and switch ctlmode)*/
int ServoKinco::Motor_Init()
{
    uint8_t nodeId = m_servo_motor.cfg.nodeId;
    if((0 == nodeId) || (nodeId > 127))
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] motor[%s] nodeId is unvaild", joint_name.c_str());
        return -1;       
    }

    /* 2023.9.19 added: canopen refactor.   ---> */
    // set_control_word_pub  = node_->create_publisher<std_msgs::msg::UInt16>(
    //     "/" + busname + "/device" + std::to_string(nodeId) + "/set_controlword", 10);
    set_vel_pub           = node_->create_publisher<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_target_velocity", 10);
    set_pos_pub           = node_->create_publisher<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_target_position", 10);

    set_control_word_sdo_pub         = node_->create_publisher<std_msgs::msg::UInt16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_controlword_sdo", 10);
    set_di_sdo_pub                   = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_digital_inputs_sdo", 10);
    set_home_offset_mode_sdo_pub     = node_->create_publisher<std_msgs::msg::UInt8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_group_homing_speed/home_offset_mode_sdo", 10);
    set_homing_switch_speed_sdo_pub  = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_group_homing_speed/homing_speeds_speed_during_search_for_switch_1_sdo", 10);
    set_homing_zero_speed_sdo_pub    = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_group_homing_speed/homing_speeds_speed_during_search_for_zero_sdo", 10);
    set_home_offset_sdo_pub          = node_->create_publisher<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_home_offset_sdo", 10);
    set_homing_acc_sdo_pub           = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_homing_accelaration_sdo", 10);
    set_homing_method_sdo_pub        = node_->create_publisher<std_msgs::msg::Int8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_homing_method_sdo", 10);
    set_mode_sdo_pub                 = node_->create_publisher<std_msgs::msg::Int8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_modes_of_operation_sdo", 10);
    set_acc_sdo_pub                  = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_profile_acceleration_sdo", 10);
    set_dec_sdo_pub                  = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_profile_deceleration_sdo", 10);
    set_p_vel_sdo_pub                = node_->create_publisher<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_profile_velocity_sdo", 10);
    set_pos_sdo_pub                  = node_->create_publisher<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/set_target_position_sdo", 10);


    mode_sub         = node_->create_subscription<std_msgs::msg::Int8>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_operation_mode_buff", 
        10, 
        std::bind(&ServoKinco::mode_sub_callback, this, std::placeholders::_1));
    errcode_sub = node_->create_subscription<std_msgs::msg::UInt16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_error_code_for_ds301", 
        10, 
        std::bind(&ServoKinco::errcode_sub_callback, this, std::placeholders::_1));
    current_pos_sub = node_->create_subscription<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_position_actual_value_", 
        10, 
        std::bind(&ServoKinco::current_pos_sub_callback, this, std::placeholders::_1));
    current_vel_sub = node_->create_subscription<std_msgs::msg::Int32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_speed_real", 
        10, 
        std::bind(&ServoKinco::current_vel_sub_callback, this, std::placeholders::_1));
    status_word_sub = node_->create_subscription<std_msgs::msg::UInt16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_statusword", 
        10, 
        std::bind(&ServoKinco::status_word_sub_callback, this, std::placeholders::_1));
    di_sub = node_->create_subscription<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_digital_inputs", 
        10, 
        std::bind(&ServoKinco::di_sub_callback, this, std::placeholders::_1));
    p_vel_sub = node_->create_subscription<std_msgs::msg::UInt32>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_profile_velocity", 
        10, 
        std::bind(&ServoKinco::p_vel_sub_callback, this, std::placeholders::_1));
    current_sub = node_->create_subscription<std_msgs::msg::Int16>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_i_q", 
        10, 
        std::bind(&ServoKinco::current_sub_callback, this, std::placeholders::_1));
    timestamp_sub = node_->create_subscription<std_msgs::msg::Float64>(
        "/" + busname + "/device" + std::to_string(nodeId) + "/get_position_actual_value_/timestamp", 
        10, 
        std::bind(&ServoKinco::timestamp_sub_callback, this, std::placeholders::_1));
    set_entry_cln    = node_->create_client<agv_srvs::srv::SetCanopenEntry>("/set_canopen_entry");

    RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] %s subscribe prefix: %s", joint_name.c_str(), std::string("/" + busname + "/device" + std::to_string(nodeId)).c_str());

    if (servo_type == JACKING) {
        target_pos_sub = node_->create_subscription<std_msgs::msg::Int32>(
            "/" + busname + "/device" + std::to_string(nodeId) + "/get_target_position", 
            10, 
            std::bind(&ServoKinco::target_pos_sub_callback, this, std::placeholders::_1));
    }

    RCLCPP_INFO(node_->get_logger(), "node[%d] enable.", nodeId);
    // motor enable
    bool enableFlage = Motor_Enable(true);
    if(!enableFlage)
    {
        return -1;
    }

    // homemachine
    if (!Motor_HomeMachine()) {
        return -1;
    }

    // ctlmode
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set position mode.", nodeId);
        if (!Motor_InitPmode())
            return -1;
    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set velocity mode.", nodeId);
        if(!Motor_InitVmode())
            return -1;
    }
    else if(m_servo_motor.cfg.ctl_mode == Immediate_Velocity)
    {
        RCLCPP_INFO(node_->get_logger(), "node[%d] set immediate velocity mode.", nodeId);
        if(!Motor_InitIVmode())
            return -1;
    }
    else
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR]unknown drive mode[%d]", m_servo_motor.cfg.ctl_mode);
        return -1;
    }

    while (!set_entry_cln->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] Interrupted while waiting for the service. Exiting.");
        return -1;
      }
      RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] service not available, waiting again...");
    }      
    motorState = Ready;        
    return 0;
}

/*Description: motor enable or disable*/
bool ServoKinco::Motor_Enable(bool onFlag)
{
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        // Motor_SetPmodeVel(0);

        /* 2023.9.19 modify: cnaopen refactor.   --->
        pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x607A, 0x00, 0x00);
        */
        std_msgs::msg::Int32 msg;
        msg.data = (int32_t)0x00;
        set_pos_sdo_pub->publish(msg);
        /* <--- modify end */

    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity || m_servo_motor.cfg.ctl_mode == Immediate_Velocity)
    {
        Motor_SetTargetVelEnc(0);
    }

    uint8_t nodeId = m_servo_motor.cfg.nodeId;

    /* 2023.9.19 modify: cnaopen refactor.   --->
    // uint16_t statusWord = pCanopen->getStatusWord(nodeId);
    */
    uint16_t statusWord = pc_.status_word;
    /* <--- modify end */


    RCLCPP_INFO(node_->get_logger(), "node %d statusWord is %d",nodeId,statusWord);
    if(!onFlag)
    {
        if((statusWord & 0x07) != 0x07)
        {
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR]motor has been disabled");
            return true;
        }

        /* 2023.9.19 modify: cnaopen refactor.   --->
        pCanopen->sdoSend(nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x06);
        */

        int timeout = 0;
        while ((statusWord & 0x07) == 0x07 && timeout < 10) {
            std_msgs::msg::UInt16 msg;
            msg.data = (uint16_t)0x06;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
            statusWord = pc_.status_word;
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] checkout status_word[0x%x] trys: %d", statusWord, timeout);
            timeout++;
        }

        if((statusWord & 0x07) == 0x07) {
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR]motor failed to be disable");
        }
        else {
            RCLCPP_WARN(node_->get_logger(), "[SERVO_MOTOR]motor was disabled successfully");               
            return false; 
        }
    }
    else
    {
        if((statusWord & 0x07) == 0x07)
        {
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR]motor has been enabled");
            return true;
        }


        uint16_t errcode = pc_.error_code;

        std_msgs::msg::UInt16 msg;
        msg.data = 0x86;
        set_control_word_sdo_pub->publish(msg);
        rclcpp::sleep_for(std::chrono::milliseconds(20));
        
        int timeout = 0;
        while (errcode && timeout < 50) {
            msg.data = 0x86;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(20));
            errcode = pc_.error_code;
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] 0x86 checkout error_code[0x%x] trys: %d", joint_name.c_str(), errcode, timeout);
            timeout++;
        }

        if(!errcode)
        {
            uint16_t statusWord = pc_.status_word;
                
            timeout = 0;
            
            msg.data = 0x06;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(20));
            statusWord = pc_.status_word;

            while ((statusWord & 0x7) != 0x1 && timeout < 50) {
                set_control_word_sdo_pub->publish(msg);
                rclcpp::sleep_for(std::chrono::milliseconds(20));
                statusWord = pc_.status_word;
                RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] 0x6 checkout status_word[0x%x] trys: %d", joint_name.c_str(), statusWord, timeout);
                timeout++;
            }

            if ((statusWord & 0x7) == 0x1) {
                timeout = 0;
                msg.data = 0x0F;
                set_control_word_sdo_pub->publish(msg);
                rclcpp::sleep_for(std::chrono::milliseconds(20));
                while ((statusWord & 0x07) != 0x07 && timeout < 50) {
                    set_control_word_sdo_pub->publish(msg);
                    rclcpp::sleep_for(std::chrono::milliseconds(20));
                    statusWord = pc_.status_word;
                    RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] 0xF checkout status_word[0x%x] trys: %d", joint_name.c_str(), statusWord, timeout);
                    timeout++;
                }
            }

            if((statusWord & 0x07) == 0x07)
            {
                // if ((servo_type == STEER || servo_type == JACKING) && m_servo_motor.cfg.ctl_mode == Profiled_Position) {
                //     std_msgs::msg::UInt16 ctrlwd_msg;
                //     ctrlwd_msg.data = (uint16_t)0x103F;
                //     set_control_word_sdo_pub->publish(ctrlwd_msg);
                //     rclcpp::sleep_for(std::chrono::milliseconds(5));
                //     set_control_word_sdo_pub->publish(ctrlwd_msg);
                //     rclcpp::sleep_for(std::chrono::milliseconds(5));
                //     set_control_word_sdo_pub->publish(ctrlwd_msg);
                //     RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] set control word 0x103F", joint_name.c_str());
                // }
                
                RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] enable succeed.", joint_name.c_str());
                return true;
            }
            else {
                RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] enable failed.", joint_name.c_str());
                return false;
            }                    
        }
        else
        {
            RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] error is sitll obtained after clear error", joint_name.c_str());
            return false;
        }
    }

    return false; 
}


/*Description: configure motor homing parameters and excute*/
bool ServoKinco::Motor_HomeMachine()
{
    uint16_t statusWord = 0;
    if(!m_servo_motor.cfg.homeNeed) //todo get 609903 check whether the system automatically returns to zero
    {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] does not need to go home", joint_name.c_str());
        return true;
    }
    if(!m_servo_motor.cfg.hModeCfg.home_mode)
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] home mode is not defined", joint_name.c_str());
        return false;
    }
    servo_modeSet(Homing);
    servo_homeMethodSet(m_servo_motor.cfg.hModeCfg.home_mode);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    servo_homeMethodSet(m_servo_motor.cfg.hModeCfg.home_mode);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    servo_homeMethodSet(m_servo_motor.cfg.hModeCfg.home_mode);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    servo_homeOffsetModeSet(0); // todo mode add cfg
    servo_homeOffsetSet(m_servo_motor.cfg.hModeCfg.homeOffset);
    servo_homeSpeedHSet(m_servo_motor.cfg.hModeCfg.velH);
    servo_homeSpeedLSet(m_servo_motor.cfg.hModeCfg.velL);
    servo_homeAccSet(m_servo_motor.cfg.hModeCfg.acc);
    rclcpp::sleep_for(std::chrono::milliseconds(10));
    servo_homeStart();
    rclcpp::sleep_for(std::chrono::milliseconds(1000));// wait homing start and status word updated
    rclcpp::Time homing_start_time = node_->now();
    do{
        /* 2023.9.19  modify: canopen refactor   --->
        statusWord = pCanopen->getStatusWord(m_servo_motor.cfg.nodeId);
        */
        statusWord = pc_.status_word;
        /* <--- modify end */
        rclcpp::sleep_for(std::chrono::milliseconds(10));

        if((node_->now() - homing_start_time).seconds() > 20) //todo timeout
        {
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] home timeout.", joint_name.c_str());
            break;
        }
    } while(!(statusWord & 0x8000));

    if(statusWord & 0x8000)
    {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] home finished.", joint_name.c_str());
    }
    else
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] home timeout.", joint_name.c_str());
        Motor_Enable(false); //disable motor
        return false;       
    }
    return true;
}

/*Description: configure the motor for Profiled Velocity mode*/
bool ServoKinco::Motor_InitVmode()
{
    servo_modeSet(Profiled_Velocity);
    if(Profiled_Velocity != Motor_GetMode())
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] failed to set velocity mode", joint_name.c_str());
        return false;
    }
    Motor_SetAcc(m_servo_motor.cfg.vModeCfg.acc);
    Motor_SetDec(m_servo_motor.cfg.vModeCfg.dec);
    Motor_SetTargetVelEnc(0);
    
    return true;
}

/*Description: configure the motor for Immediate Velocity mode*/
bool ServoKinco::Motor_InitIVmode()
{
    servo_modeSet(Immediate_Velocity);
    if(Immediate_Velocity != Motor_GetMode())
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] failed to set immediate velocity mode", joint_name.c_str());
        return false;
    }
    Motor_SetAcc(m_servo_motor.cfg.vModeCfg.acc);
    Motor_SetDec(m_servo_motor.cfg.vModeCfg.dec);
    Motor_SetTargetVelEnc(0);
    
    return true;
}

/*Description: configure the motor for Profiled Position mode*/
bool ServoKinco::Motor_InitPmode()
{
    servo_modeSet(Profiled_Position);
    if(Profiled_Position != Motor_GetMode())
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] failed to set position mode", joint_name.c_str());
        return false;
    }
    Motor_SetAcc(m_servo_motor.cfg.pModeCfg.acc);
    Motor_SetDec(m_servo_motor.cfg.pModeCfg.dec);
    Motor_SetPmodeVel(m_servo_motor.cfg.pModeCfg.vel);

    std_msgs::msg::UInt16 ctrlwd_msg;
    ctrlwd_msg.data = (uint16_t)0x103F;
    set_control_word_sdo_pub->publish(ctrlwd_msg);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    set_control_word_sdo_pub->publish(ctrlwd_msg);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    set_control_word_sdo_pub->publish(ctrlwd_msg);

    auto set_entry_request = std::make_shared<agv_srvs::srv::SetCanopenEntry::Request>();
    set_entry_request->bus_name = std::string("can0");
    set_entry_request->node_id = m_servo_motor.cfg.nodeId;
    set_entry_request->index = 0x6040;
    set_entry_request->sub_index = 0x0;
    set_entry_request->value = (uint16_t)0x103F;
    try {
        using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::SetCanopenEntry>::SharedFuture;
        auto response_received_callback = [this](ServiceResponseFuture future) {
            if (future.get()->success) {
                RCLCPP_INFO(node_->get_logger(), 
                    "[ServoKinco] call canopen service success. set control word 0x6040 value 0x103F.");
            } else {
                RCLCPP_WARN(node_->get_logger(), 
                    "[ServoKinco] call canopen service fail.");
            }
        };
        auto result = set_entry_cln->async_send_request(set_entry_request, response_received_callback);
    } catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "[ServoKinco] call canopen service error: %s", e.what());
        return false;
    }
    return true;
}

/*Description: switch motor mode, currently only supported Profiled Position mode and Profiled Velocity mode*/
bool ServoKinco::Motor_SwitchMode(int8_t mode)
{
    switch(mode)
    {
        case Profiled_Position:
            Motor_InitPmode();
            break;
        case Profiled_Velocity:
            Motor_InitVmode();
            break;
        case Immediate_Velocity:
            Motor_InitIVmode();
            break;
        case Homing:
            break;
        default:
            RCLCPP_WARN(node_->get_logger(), "servo[%s] undefined control mode %d", joint_name.c_str(), mode);
            break;
    }
    int8_t actualCtlMode = Motor_GetMode();
    if(actualCtlMode != mode)
    {
        RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] failed to set the ctl mode", joint_name.c_str());
        return false;
    }
    return true;
}

/*Description: set motor acceleration*/
void ServoKinco::Motor_SetAcc(uint32_t acc)
{
    std_msgs::msg::UInt32 msg;
    msg.data = acc;
    set_acc_sdo_pub->publish(msg);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    set_acc_sdo_pub->publish(msg);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    set_acc_sdo_pub->publish(msg);
}

/*Description: set motor deceleration*/
void ServoKinco::Motor_SetDec(uint32_t dec)
{
    std_msgs::msg::UInt32 msg;
    msg.data = dec;
    set_dec_sdo_pub->publish(msg);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    set_dec_sdo_pub->publish(msg);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    set_dec_sdo_pub->publish(msg);
}

/*Description: set the speed for Profiled Position mode*/
void ServoKinco::Motor_SetPmodeVel(uint32_t vel)
{
    std_msgs::msg::UInt32 msg;
    msg.data = vel;
    set_p_vel_sdo_pub->publish(msg);
    rclcpp::sleep_for(std::chrono::milliseconds(5));
    
    auto set_entry_request = std::make_shared<agv_srvs::srv::SetCanopenEntry::Request>();
    set_entry_request->bus_name = std::string("can0");
    set_entry_request->node_id = m_servo_motor.cfg.nodeId;
    set_entry_request->index = 0x6081;
    set_entry_request->sub_index = 0x0;
    set_entry_request->value = vel;

    try {
        using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::SetCanopenEntry>::SharedFuture;
        auto response_received_callback = [this](ServiceResponseFuture future) {
            if (future.get()->success) {
                RCLCPP_INFO(node_->get_logger(), 
                    "[ServoKinco] call canopen service success. set control word 0x6040 value 0x103F.");
            } else {
                RCLCPP_WARN(node_->get_logger(), 
                    "[ServoKinco] call canopen service fail.");
            }
        };
        auto result = set_entry_cln->async_send_request(set_entry_request, response_received_callback);
    } catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "[ServoKinco] call canopen service error: %s", e.what());
        return;
    }

    RCLCPP_INFO(node_->get_logger(), "[ServoKinco] set position mode vel: %d", vel);
}

/*Description: set the speed for Profiled Velocity mode, vel is expressed in rad/s*/
void ServoKinco::Motor_SetTargetVelRad(double vel)
{
    /* 2023.8.17 add begin: add motor state condition  ---> */
    if(Ready != Motor_GetState())
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoKinco] servo[%s] state[%d] not Ready !!!", joint_name.c_str(), Motor_GetState());
        return;
    }
    /* <--- add end */

    if(Motor_GetMode() != Profiled_Velocity && Motor_GetMode() != Immediate_Velocity)
    {
        return;
    } 
    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoKinco] servo[%s] reduction_ratio||encoder_resolution is 0.", joint_name.c_str());
        return;
    }

    if (std::isnan(vel) || abs(vel) > 1e9) {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[ServoKinco] bad vel value: %.4f", vel);
        Motor_SetTargetVelEnc(0);
        return;
    }
    
    //RCLCPP_INFO(node_->get_logger(), "vel is %lf",vel);

    /* 2023.8.17 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        vel /= rad_to_meter;
    } else {
        // continuous: do nothing
    }

    /* <--- and end */
    // RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 0.5, "[ServoKinco] set target vel rad: %.4f", vel);
    const double temp  = 30 / M_PI * m_servo_motor.cfg.reduction_ratio * 512 * m_servo_motor.cfg.encoder_resolution  / 1875;
    int targetVel =  vel * temp;

    /* 2023.8.17 modify begin:  use setTargetVelEnc instead  --->
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_60FF00, targetVel);
    <--- old code  |  new code  ---> */
    Motor_SetTargetVelEnc(targetVel);
    /* <--- modify end */
}

/*Description: set the speed for Profiled Velocity mode, vel is expressed in encoder*/
void ServoKinco::Motor_SetTargetVelEnc(int vel)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_60FF00, vel);
    */
    RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
        500, "[ServoKinco] servo[%s] set_target vel enc: %d", joint_name.c_str(), vel);
    std_msgs::msg::Int32 msg;
    msg.data = vel;
    set_vel_pub->publish(msg);
    /* <--- modify end */
}

/*Description: set the target pos for Absolute position control mode, pos is expressed in encoder*/
void ServoKinco::Motor_SetTargetPosEnc(int pos)
{
    if(Ready != Motor_GetState())
    {
        return;
    }
    int encoder = servo_encoderGet();
    // RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR]get encoder %d",encoder);
    if(abs(pos - encoder) < 100)
    {
        return;
    }

    std_msgs::msg::Int32 pos_msg;
    pos_msg.data = pos;
    set_pos_pub->publish(pos_msg);
}

/*Description: set the target pos for Absolute position control mode, pos is expressed in rad*/
void ServoKinco::Motor_SetTargetPosRad(double pos)
{
    if(Ready != Motor_GetState())
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[SERVO_MOTOR] servo[%s] state[%d] not Ready !!!", joint_name.c_str(), Motor_GetState());
        return;
    }
    if(Motor_GetMode() != Profiled_Position)
    {
        // RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] motor mode[%d] not Profiled_Position !!!", Motor_GetMode());
        return;
    }
    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
            1000, "[SERVO_MOTOR] servo[%s] reduction_ratio = 0 || encoder_resolution = 0", joint_name.c_str());
        return;
    }

     /* 2023.8.17 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        pos /= rad_to_meter;
    } else {
        // continuous: do nothing
    }

    /* <--- and end */


    //int targetEnc = pos / 2 / M_PI * m_servo_motor.cfg.encoder_resolution * m_servo_motor.cfg.reduction_ratio;
    int encoder = servo_encoderGet();
    double radToEnc = 2.0 * M_PI / m_servo_motor.cfg.encoder_resolution / m_servo_motor.cfg.reduction_ratio;
    int targetEnc = 0;
    if (servo_type == STEER) {
        targetEnc = pos /radToEnc + zero_point_correction;
        // RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 1000, "[ServoKinco] %s set pos[%.4f]  enc[%d]  current[%d]", joint_name.c_str(), pos, targetEnc, encoder);
        Motor_SetTargetPosEnc(targetEnc);
    }
    else {
        if (abs(radToEnc) > 1e-6)
            targetEnc = pos / radToEnc;
        else 
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] radToEnc zero: %f", joint_name.c_str(), radToEnc);
    }

    // RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 0.5, "[SERVO_MOTOR] get encoder: %d  targetEnc: %d",encoder, targetEnc);
    if(abs(targetEnc - encoder) < 100)
    {
        return;
    }
    Motor_SetTargetPosEnc(targetEnc);
}

/*Description: set the target pos for relative position control mode, pos is expressed in encoder*/
void ServoKinco::servo_PRelMove(int data)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_607A00, data);
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_604000, 0x4F);
    rclcpp::sleep_for(std::chrono::milliseconds(30));
    pCanopen->pdoSend(m_servo_motor.cfg.nodeId, E_RPDO_604000, 0x5F);
    */
    std_msgs::msg::Int32 pos_msg;
    pos_msg.data = data;
    set_pos_pub->publish(pos_msg);

    std_msgs::msg::UInt16 ctrlwd_msg;
    ctrlwd_msg.data = (uint16_t)0x4F;
    set_control_word_pub->publish(ctrlwd_msg);

    rclcpp::sleep_for(std::chrono::milliseconds(30));
    ctrlwd_msg.data = (uint16_t)0x5F;
    set_control_word_pub->publish(ctrlwd_msg);
    /* <---  modify end */
}


/*Description: get the motor speed ignore control mode, pos is expressed in rad per second*/
double ServoKinco::Motor_GetCurrentVel()
{
    int vel;
    double retVel;
    const double temp  = 30 / M_PI * m_servo_motor.cfg.reduction_ratio * 512 * m_servo_motor.cfg.encoder_resolution  / 1875;
    
    vel = servo_VvelGet();
    retVel = vel / temp;

    /* 2023.8.18 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        retVel *= rad_to_meter;
    } else {
        // continuous: do nothing
    }
    
    return retVel;
}

/*Description: get the motor speed ignore control mode*/
int ServoKinco::servo_velGet()
{
    if(m_servo_motor.cfg.ctl_mode == Profiled_Position)
    {
        return servo_PvelGet();
    }
    else if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity || m_servo_motor.cfg.ctl_mode == Immediate_Velocity)
    {
        return servo_VvelGet();
    }
    else
        return 0;
}

/*Description: get the motor speed in position control mode*/
uint32_t ServoKinco::servo_PvelGet()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getPvel(m_servo_motor.cfg.nodeId);
    */
    return pc_.profile_vel;
    /* <--- modify end */
}

/*Description: get the motor speed in velocity control mode*/
int ServoKinco::servo_VvelGet()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getVvel(m_servo_motor.cfg.nodeId, servo_type);
    */
    return pc_.current_vel;
    /* <--- modify end */
}

/*Description: get the motor position,unit is rad*/
double ServoKinco::Motor_GetCurrentPos()
{
    int encoder = servo_encoderGet();
    if((m_servo_motor.cfg.reduction_ratio < 1e-6) || !m_servo_motor.cfg.encoder_resolution)
    {
        return 0;
    }
    const double encTorad = m_servo_motor.cfg.encoder_resolution * m_servo_motor.cfg.reduction_ratio / 2.0 / M_PI;
    double pos = encoder / encTorad;

    /* 2023.8.18 add begin: refactor set pos/vel interface  ---> */
    if (servo_type == JACKING) {
        // prismatic
        pos *= rad_to_meter;
    } 
    else if (servo_type == STEER) {
        pos = (encoder - zero_point_correction) / encTorad;
    }
    else {
        // continuous: do nothing
    }

    return pos;
}

/*Description: get the motor encoder*/
int ServoKinco::servo_encoderGet()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getEnc(m_servo_motor.cfg.nodeId);
    */
    return pc_.current_pos;
    /* <--- modify end */
}

/*Description: get the motor status word*/
uint16_t ServoKinco::Motor_GetStatusWord()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getStatusWord(m_servo_motor.cfg.nodeId);
    */
    return pc_.status_word;
    /* <--- modify end */
}

double ServoKinco::Motor_GetCurrent()
{
    double result = 0.0;
    result = pc_.current / (2048/(ipeak/1.414));
    return result;
}

std::string ServoKinco::Motor_Err2userErr(const uint16_t& motorErr)
{
    std::string errInfo;
    switch(motorErr)
    {
        case 0x0000: errInfo = "no err"; break;
        case 0x7380: errInfo = "Encoder ABZ signal error"; break;
        case 0x7331: errInfo = "Encoder communication error"; break;
        case 0x7381: errInfo = "Encoder UVW signal error"; break;
        case 0x7320: errInfo = "Encoder internal error"; break;
        case 0x7305: errInfo = "Encoder count error"; break;
        case 0x7330: errInfo = "Encoder CRC check error"; break;
        case 0x4210: errInfo = "Driver overtemperature"; break;
        case 0x3210: errInfo = "Driver voltage is too high"; break;
        case 0x3220: errInfo = "Driver voltage is too low."; break;
        case 0x2320: errInfo = "Driver output short circuit"; break;
        case 0x7110: errInfo = "Driver brake resistance is abnormal"; break;
        case 0x8611: errInfo = "Following error"; break;
        case 0x5112: errInfo = "The logical voltage is too low."; break;
        case 0x2350: errInfo = "Motor overload"; break;
        case 0x8A80: errInfo = "The input pulse frequency is too high"; break;
        case 0x4310: errInfo = "Driver overtemperature"; break;
        case 0x7122: errInfo = "Motor UVW logic combination error"; break;
        case 0x6310: errInfo = "EEPROM data error"; break;
        case 0x5210: errInfo = "Current sensor fault"; break;
        case 0x6010: errInfo = "The watchdog reports error"; break;
        case 0x6011: errInfo = "Invalid interrupt occur"; break;
        case 0x7400: errInfo = "MCU fault"; break;
        case 0x6320: errInfo = "Motor type error"; break;
        case 0x6321: errInfo = "Motor phase loss"; break;
        case 0x5443: errInfo = "The alarm is pre-enabled"; break;
        case 0x5442: errInfo = "Positive limit reported error"; break;
        case 0x5441: errInfo = "Negative limit reported error"; break;
        case 0x6012: errInfo = "SPI communication failure"; break;
        case 0x8100: errInfo = "Communication error"; break;
        case 0x81FF: errInfo = "Communication timeout"; break;
        case 0x8A81: errInfo = "Full closed loop check error"; break;
        case 0x7382: errInfo = "Master encoder ABZ error"; break;
        case 0x7306: errInfo = "Master encoder count error"; break;
        case 0x5530: errInfo = "device software reset"; break;
        case 0xFF42: errInfo = "Battery is discharged"; break;
        case 0xFF45: errInfo = "Current Gain under acquisition"; break;
        case 0xFF50: errInfo = "Forward and Reverse request direction together"; break;
        case 0x5301: errInfo = "Pal Circuit KO"; break;
        case 0x1000: errInfo = "generic warning"; break;
        case 0xFFF4: errInfo = "When the input B3 is opened"; break;
        case 0x3223: errInfo = "A11output broken/DC link under-voltage No.3 (manufacturer specific)"; break;
        case 0x4211: errInfo = "sensor temperatur device broken (manufacturer specific)"; break;
        default:     errInfo = "unknown warning"; break;
    }
    return errInfo;
}

/*Description: get the motor error code from object 603f*/
uint16_t ServoKinco::Motor_GetErrCode()
{ 
    /* 2023.9.19  modify: canopen refactor.   --->
    uint16_t errCode = errCode = pCanopen->getErrCode(m_servo_motor.cfg.nodeId);
    */
    uint16_t errCode = pc_.error_code;
    /* <--- modify end */
    if(Ready == Motor_GetState())
    {
        if(errCode)
        {
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()),
                1000, "[ServoKinco] errcode: %d", errCode);
            motorState = Error;
        }
    }
    else {
        if (Error == Motor_GetState() && errCode == 0) {
            motorState = Ready;
            RCLCPP_WARN_THROTTLE(node_->get_logger(), *(node_->get_clock()),
                1000, "[ServoKinco] errcode clear");   
        }
    }

    return errCode;
}

/*Description: get the motor error code from object 603f*/
uint32_t ServoKinco::servo_DiStatusGet()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getDiStatus(m_servo_motor.cfg.nodeId, servo_type);
    */
    return pc_.digital_input;
    /* <--- modify end */
}

/*Description: get the status of the drive digital input signal*/
void ServoKinco::servo_ErrClear()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x86);
    */
    std_msgs::msg::UInt16 msg;
    msg.data = (uint16_t)0x86;
    set_control_word_sdo_pub->publish(msg);
    /* <--- modify end */
}


/*Description: motor electric brake control,only zapi motor are supported*/
void ServoKinco::Motor_Brake(bool on)
{

}

/*Description: shutdown servo*/
void ServoKinco::Motor_Shutdown()
{
    return;
    /* 2023.9.19  delete: canopen refactor.   --->
    pCanopen->nmtSend(m_servo_motor.cfg.nodeId, Canopen::State::Stopped);
    */
}

bool ServoKinco::Motor_Halt()
{
    motorState = Halt;
    std_msgs::msg::UInt16 msg;
    msg.data = 0x0B;
    set_control_word_sdo_pub->publish(msg);
    rclcpp::sleep_for(std::chrono::milliseconds(20));

    if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity || m_servo_motor.cfg.ctl_mode == Immediate_Velocity) {
        Motor_SetTargetVelEnc(0);
    }

    int timeout = 0;
    uint16_t status_word = pc_.status_word;

    while ((status_word & 0x07) == 0x07 && timeout < 50) {
        set_control_word_sdo_pub->publish(msg);
        rclcpp::sleep_for(std::chrono::milliseconds(20));
        status_word = pc_.status_word;
        timeout++;
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] checkout status_word[0x%x] trys: %d", joint_name.c_str(), status_word, timeout);
    }

    if ((status_word & 0x07) != 0x07) {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] disable succeed.", joint_name.c_str());
        return true;
    }
    else {
        RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] disable failed.", joint_name.c_str());
        return false;
    }
}

/*Description: reset servo error and enable servo*/
bool ServoKinco::Motor_Recover()
{
    uint8_t nodeId = m_servo_motor.cfg.nodeId;
    if (motorState != Recover)
    {
        motorState = Recover;

        if(m_servo_motor.cfg.ctl_mode == Profiled_Velocity || m_servo_motor.cfg.ctl_mode == Immediate_Velocity) {
            Motor_SetTargetVelEnc(0);
        }
        
        uint16_t errcode = pc_.error_code;

        std_msgs::msg::UInt16 msg;
        msg.data = 0x86;
        set_control_word_sdo_pub->publish(msg);
        rclcpp::sleep_for(std::chrono::milliseconds(20));
        
        int timeout = 0;
        while (errcode && timeout < 50) {
            msg.data = 0x86;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(20));
            errcode = pc_.error_code;
            RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] 0x86 checkout error_code[0x%x] trys: %d", joint_name.c_str(), errcode, timeout);
            timeout++;
        }

        if(!errcode)
        {
            uint16_t statusWord = pc_.status_word;
                
            timeout = 0;
            
            msg.data = 0x06;
            set_control_word_sdo_pub->publish(msg);
            rclcpp::sleep_for(std::chrono::milliseconds(20));
            statusWord = pc_.status_word;

            while ((statusWord & 0x7) != 0x1 && timeout < 50) {
                set_control_word_sdo_pub->publish(msg);
                rclcpp::sleep_for(std::chrono::milliseconds(20));
                statusWord = pc_.status_word;
                RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] 0x6 checkout status_word[0x%x] trys: %d", joint_name.c_str(), statusWord, timeout);
                timeout++;
            }

            if ((statusWord & 0x7) == 0x1) {
                timeout = 0;
                msg.data = 0x0F;
                set_control_word_sdo_pub->publish(msg);
                rclcpp::sleep_for(std::chrono::milliseconds(20));
                while ((statusWord & 0x07) != 0x07 && timeout < 50) {
                    set_control_word_sdo_pub->publish(msg);
                    rclcpp::sleep_for(std::chrono::milliseconds(20));
                    statusWord = pc_.status_word;
                    RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] 0xF checkout status_word[0x%x] trys: %d", joint_name.c_str(), statusWord, timeout);
                    timeout++;
                }
            }

            if((statusWord & 0x07) == 0x07)
            {
                if ((servo_type == STEER || servo_type == JACKING) && m_servo_motor.cfg.ctl_mode == Profiled_Position) {
                    std_msgs::msg::UInt16 ctrlwd_msg;
                    ctrlwd_msg.data = (uint16_t)0x103F;
                    set_control_word_sdo_pub->publish(ctrlwd_msg);
                    rclcpp::sleep_for(std::chrono::milliseconds(5));
                    set_control_word_sdo_pub->publish(ctrlwd_msg);
                    rclcpp::sleep_for(std::chrono::milliseconds(5));
                    set_control_word_sdo_pub->publish(ctrlwd_msg);
                    RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] set control word 0x103F", joint_name.c_str());
                }
                
                motorState = Ready;
                RCLCPP_INFO(node_->get_logger(), "[SERVO_MOTOR] servo[%s] enable succeed.", joint_name.c_str());
            }                    
        }
        else
        {
            motorState = Error;
            RCLCPP_ERROR(node_->get_logger(), "[SERVO_MOTOR] servo[%s] error is sitll obtained after clear error", joint_name.c_str());
            return false;
        }
    }
    return true;
}


void ServoKinco::servo_homeOffsetModeSet(uint8_t mode)
{
    if(mode > 1)
    {
        return;
    }

    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 1, 0x6099, 0x05, mode);    
    */
    std_msgs::msg::UInt8 msg;
    msg.data = mode;
    set_home_offset_mode_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoKinco::servo_homeOffsetSet(int homeOffset)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x607c, 0x00, homeOffset);
    */
    std_msgs::msg::Int32 msg;
    msg.data = homeOffset;
    set_home_offset_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoKinco::servo_homeMethodSet(int homeMethod)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 1, 0x6098, 0x00, homeMethod);
    */
    std_msgs::msg::Int8 msg;
    msg.data = homeMethod;
    set_homing_method_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoKinco::servo_homeSpeedHSet(uint32_t homeSpeedH)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6099, 0x01, homeSpeedH);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = homeSpeedH;
    set_homing_switch_speed_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoKinco::servo_homeSpeedLSet(uint32_t homeSpeedL)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x6099, 0x02, homeSpeedL);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = homeSpeedL;
    set_homing_zero_speed_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoKinco::servo_homeAccSet(uint32_t homeAcc)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x609A, 0x00, homeAcc);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = homeAcc;
    set_homing_acc_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoKinco::servo_homeStart()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x0F);
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 2, 0x6040, 0x00, 0x1F); 
    */
    std_msgs::msg::UInt16 msg;
    msg.data = 0x0F;
    set_control_word_sdo_pub->publish(msg);
    msg.data = 0x1F;
    set_control_word_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoKinco::servo_readDiStatus()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 4, 0x60FD, 0x00, 0x00);
    */
    std_msgs::msg::UInt32 msg;
    msg.data = 0x0;
    set_di_sdo_pub->publish(msg);
    /* <--- modify end */
}

void ServoKinco::servo_modeSet(int8_t mode)
{
    /* 2023.9.19  modify: canopen refactor.   --->
    pCanopen->sdoSend(m_servo_motor.cfg.nodeId, SDO_W_FUNC, 1, 0x6060, 0x00, mode);
    */

    int timeout = 0;
    while (Motor_GetMode() != mode && timeout < 20) {
        std_msgs::msg::Int8 msg;
        msg.data = mode;
        set_mode_sdo_pub->publish(msg);
        rclcpp::sleep_for(std::chrono::milliseconds(500));
        
        RCLCPP_INFO(node_->get_logger(), "set mode: %d  current: % d  trys: %d", mode, Motor_GetMode(), timeout);
        timeout++;
    }
    /* <--- modify end */
}

int8_t ServoKinco::Motor_GetMode()
{
    /* 2023.9.19  modify: canopen refactor.   --->
    return pCanopen->getActualCtlMode(m_servo_motor.cfg.nodeId);
    */
    return pc_.mode;
}


MotorState ServoKinco::Motor_GetState() 
{
    return motorState;
}


void ServoKinco::mode_sub_callback(const std_msgs::msg::Int8::ConstSharedPtr& msg)
{
    pc_.mode = msg->data;
}

void ServoKinco::errcode_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg)
{
    pc_.error_code = msg->data;
}

void ServoKinco::current_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg)
{
    pc_.current_pos = msg->data;
}

void ServoKinco::current_vel_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg)
{
    pc_.current_vel = msg->data;
}

void ServoKinco::status_word_sub_callback(const std_msgs::msg::UInt16::ConstSharedPtr& msg)
{
    pc_.status_word = msg->data;
}

void ServoKinco::di_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg)
{
    pc_.digital_input = msg->data;
}

void ServoKinco::p_vel_sub_callback(const std_msgs::msg::UInt32::ConstSharedPtr& msg)
{
    pc_.profile_vel = msg->data;
}

void ServoKinco::current_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr& msg)
{
    pc_.current = msg->data;
}

void ServoKinco::target_pos_sub_callback(const std_msgs::msg::Int32::ConstSharedPtr& msg)
{
    pc_.target_pos = msg->data;
}

void ServoKinco::timestamp_sub_callback(const std_msgs::msg::Float64::ConstSharedPtr& msg)
{
    pc_.timestamp = msg->data;
}

}